#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/utils.h"
#include <chrono>

using namespace std::chrono_literals;

class TFListener : public rclcpp::Node{
private:
    std::shared_ptr<tf2_ros::TransformListener> m_listener;
    std::shared_ptr<tf2_ros::Buffer> m_buffer;
    rclcpp::TimerBase::SharedPtr m_timer;

    void getTransform(){
        try
        {
            const auto transform = m_buffer->lookupTransform("base_link","target_point",this->get_clock()->now(),rclcpp::Duration::from_seconds(1.0f));
            auto translation = transform.transform.translation;
            auto rotation = transform.transform.rotation;
            double y,p,r;
            tf2::getEulerYPR(rotation,y,p,r);
            RCLCPP_INFO(this->get_logger(),"平移：%f,%f,%f",translation.x,translation.y,translation.z);
            RCLCPP_INFO(this->get_logger(),"旋转%f,%f,%f",y,p,r);
        }
        catch(const std::exception& e)
        {
            RCLCPP_INFO(this->get_logger(),"%s",e.what());
        }
        
        
    }
public:
    TFListener():Node("tf_listener"){
        this->m_buffer = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        this->m_listener = std::make_shared<tf2_ros::TransformListener>(*(m_buffer),this);
        // this->publish_tf();
        m_timer = this->create_wall_timer(3s,std::bind(&TFListener::getTransform,this));
    }
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node = std::make_shared<TFListener>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}